%author: wxj233
%time: 2023.11.12 10:00
%function:  特征点

classdef FeaturePoint < handle
    %FEATUREPOINT 特征点
    %   用于记录每一个轨迹的特征点
    
    properties
        id  % 特征点唯一标识
        points  % 特征点轨迹,其中points的结构为[t, x, vx, y, vy, id, cluster],id为真实目标id
        Xk_e  % 特征点当前估计值
        Pk_ee  % 特征点估计误差协方差
        lifeMax  % 特征点最大生命值
        isValidAss  % 是否为有效关联,每当与一个真实量测关联跟新该值赋值true，否则为false

        fplife  % 特征点生命值，从上次有效量测以来，每记忆一次减一，有效量测一次恢复最大生命值
        mNum  % 量测数量(measure Number),有效量测次数,每有效关联一次加一
        track  % 该特征点所属轨迹

        assP  % 用于状态跟新的关联点[t, x, y, id, cluster],没有则赋值为[]
    end
    
    methods
        function obj = FeaturePoint(lifeMax, points, Xk_e, Pk_ee, track)
            %FEATUREPOINT 构造此类的实例
            % lifeMax: 特征点最大生命值
            % points: 初始轨迹，长度为2[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye; ...]
            % Xk_e: 初始状态
            % Pk_ee: 初始估计误差协方差矩阵
            % track: 当前特征点所属轨迹
            obj.id = obj.geId();
            obj.lifeMax = lifeMax;
            obj.points = points;
            obj.Xk_e = Xk_e;
            obj.Pk_ee = Pk_ee;
            obj.track = track;
            obj.assP = [];

            obj.isValidAss = true;
            obj.mNum = 2;
            obj.fplife = lifeMax;
        end

        function costs = costs(obj, points, inDoor, dr, dtheta, dvr)
            % 计算某些点属于该特征点轨迹的代价
            % points：待计算的点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % dr: 距离量测方差
            % dtheta: 角度量测方差
            % dvr: 径向速度量测方差
            % inDoor: 落入波门中的概率
            % return: 代价值，（这个地方是每个点的概率密度值，实际上是收益值）

            % 量测误差协方差矩阵
            R = [dr, 0, 0;
                 0, dtheta, 0;
                 0, 0, dvr];
            Xk_p = obj.Xk_e;  % 下一个状态预测值（因为状态已经换成了预测状态）
            Pk_pe = obj.Pk_ee;  % 预测误差协方差矩阵

            [Zk_p, Pz, ~] = UT(@obj.h, Xk_p, Pk_pe, 3);
            Vks = points(:, [2,3,4])' - Zk_p;  % 新息（量测残差）
            Sk = Pz+R;  % 新息协方差

            costs = [];  % 代价矩阵,取距离为代价
            for i = 1:1:size(Vks, 2)
                Vk = Vks(:, i);
                chi = Vk'/Sk*Vk;  %  三个自由度的卡方分布
                p = 0;
                if chi < chi2inv(inDoor, 3)
                    p = 1/((2*pi)^(3/2)*sqrt(det(Sk))) * exp(-0.5*chi);
                end
                costs = [costs, p];
            end
        end


        function Zk = h(~, Xk_e)
          % 非线性变换函数
          % Xk_e: 原状态[x, vx, y, vy]'
          x = Xk_e(1,1);
          vx = Xk_e(2,1);
          y = Xk_e(3,1);
          vy = Xk_e(4,1);
        
          r = sqrt(x*x+y*y);
          theta = atan2(y, x);
          vr = [x, y]*[vx, vy]'/norm([x, y]);
        
          Zk = [r, theta, vr]';
        end

    end

    methods (Static)
        function id = geId(varargin)
          % 获取特征点唯一标识
          persistent ID;
          if size(varargin, 2) == 1
            ID = varargin{1};
            return;
          end
          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end


end

